#include "app_pid.h"

inline float caculate_pid(PID_para_t *pid, float target, float current)
{
    float pidout;
    pid->error = target -current;
    if(pid->error>0)
    {
        if(pid->error>5.0f)
        {
            pid->P = pid->P_big_plus;
            pid->I = pid->I_big_plus;
            pid->D = pid->D_big_plus;
        }
        else if (pid->error>0.5f)
        {
            pid->P = pid->P_mid_plus;
            pid->I = pid->I_mid_plus;
            pid->D = pid->D_mid_plus;
        }
        else
        {
            pid->P = pid->P_sml_plus;
            pid->I = pid->I_sml_plus;
            pid->D = pid->D_sml_plus;
        }
    }
    else
    {
         if(pid->error<-5.0f)
        {
            pid->P = pid->P_big_minus;
            pid->I = pid->I_big_minus;
            pid->D = pid->D_big_minus;
        }
        else if (pid->error<-0.5f)
        {
            pid->P = pid->P_mid_minus;
            pid->I = pid->I_mid_minus;
            pid->D = pid->D_mid_minus;
        }
        else
        {
            pid->P = pid->P_sml_minus;
            pid->I = pid->I_sml_minus;
            pid->D = pid->D_sml_minus;
        }
    }
    pid->integration += pid->error*pid->I;
    pid->d_error = pid->last_error - pid->error;
    pid->proportion = pid->error*pid->P;
    pid->differential = pid->d_error * pid->D;
    pid->last_error = pid->error;
    pidout = pid->proportion+pid->integration+pid->differential;
    if(pid->integration>MAXIntegration)
    {
        pid->integration = MAXIntegration;
    }
    if(pid->integration<MINIntegration)
    {
        pid->integration=MINIntegration;
    }
    if(pidout>MAXOUTPUT)
    {
        pidout= MAXOUTPUT;
    }
    if(pidout<MINOUTPUT)
    {
        pidout = MINOUTPUT;
    }
    return pidout;
}

void pid_object_init(void)
{
    
}













